#include "pid.h"
#include "motorencoder.h" 

/**
 * @brief 初始化PID参数
 * 
 * @param pid pid参数结构体指针
 * @param mode 增量式DELTA_PID、位置式POSITION_PID
 * @param p 比例
 * @param i 积分
 * @param d 微分
 */
void PID_Init(PID_t *pid, uint8_t mode, float p, float i, float d)
{
    pid->pid_mode = mode;
    pid->p = p;
    pid->i = i;
    pid->d = d;
}

/**
 * @brief PID计算输出值
 * 
 * @param pid pid结构体指针
 * @param target 目标值
 * @param now 当前值
 * @return float pid计算输出值
 */
float PID_MotorCal(PID_t *pid, float target, float now)
{
    uint8_t dir = 1;
    
    pid->target = target;
    pid->now = now;// 编码器读取的速度
    PID_Cal(pid);
    // printf("%f\n", pid->out);
    return pid->out;
}

/**
 * @brief 计算pid参数
 * 
 * @param pid 
 */
void PID_Cal(PID_t *pid)
{
    // 计算当前偏差
    pid->error[0] = pid->target - pid->now;

    // 计算输出
    if (pid->pid_mode == DELTA_PID)  // 增量式
    {
        pid->pout = pid->p * (pid->error[0] - pid->error[1]);
        pid->iout = pid->i * pid->error[0];
        pid->dout = pid->d * (pid->error[0] - 2 * pid->error[1] + pid->error[2]);
        pid->out += pid->pout + pid->iout + pid->dout;
    }
    else if (pid->pid_mode == POSITION_PID)  // 位置式
    {
        pid->pout = pid->p * pid->error[0];
        pid->iout += pid->i * pid->error[0];
        pid->dout = pid->d * (pid->error[0] - pid->error[1]);
        pid->out = pid->pout + pid->iout + pid->dout;
    }

    // 记录前两次偏差
    pid->error[2] = pid->error[1];
    pid->error[1] = pid->error[0];

    // 输出限幅
    if (pid->out >= PID_MotorMAXOUT)	
        pid->out = PID_MotorMAXOUT;
    if (pid->out <= 0)	
        pid->out = 0;
}
